// 测试 IM600 数据包解析
// 官方数据包: 11 FF 0F 37 BD 18 00 FF FF 00 00 00 00 F8 07 5E FF 2E 00 00 00 00 00 00 00 00 00 00 00 00 00 CD 08 00 00 00 00 00 00 DE 41 F8 BC C0 C4 6F C0 7C CB 5E C3 C6 F2 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 FF FF 0F 0A 01

public class TestIM600Parse {
    
    // 转换系数
    private static final double COEF_ACCEL = 0.00478515625;
    private static final double COEF_GYRO = 0.06103515625;
    private static final double COEF_EULER = 0.005493164625;
    
    public static void main(String[] args) {
        // 官方数据包（去除空格）
        String hexData = "11FF0F37BD1800FFFF00000000F8075EFF2E0000000000000000000000CD08000000000000DE41F8BCC0C46FC07CCB5EC3C6F20000000000000000000000000000FFFF0F0A01";
        
        byte[] data = hexStringToByteArray(hexData);
        
        System.out.println("========== 数据包解析 ==========");
        System.out.printf("总长度: %d 字节\n", data.length);
        System.out.println();
        
        int offset = 0;
        
        // 1. 包类型
        byte packetType = data[offset++];
        System.out.printf("[0] 包类型: 0x%02X\n", packetType);
        
        // 2. 订阅ID (2字节, 小端)
        int subscriptionId = readUInt16LE(data, offset);
        offset += 2;
        System.out.printf("[1-2] 订阅ID: 0x%04X (%d)\n", subscriptionId, subscriptionId);
        System.out.printf("  Bit6 (欧拉角): %s\n", (subscriptionId & 0x0040) != 0 ? "✓" : "✗");
        System.out.println();
        
        // 3. 时间戳 (4字节, 小端)
        long timestamp = readUInt32LE(data, offset);
        offset += 4;
        System.out.printf("[3-6] 时间戳: %d\n", timestamp);
        System.out.println();
        
        // 4. Bit0: 无重力加速度 (6字节)
        if ((subscriptionId & 0x0001) != 0) {
            System.out.printf("[%d-%d] Bit0: 无重力加速度 (跳过)\n", offset, offset + 5);
            offset += 6;
        }
        
        // 5. Bit1: 含重力加速度 (6字节)
        if ((subscriptionId & 0x0002) != 0) {
            int rawAccelX = readInt16LE(data, offset);
            offset += 2;
            int rawAccelY = readInt16LE(data, offset);
            offset += 2;
            int rawAccelZ = readInt16LE(data, offset);
            offset += 2;
            
            double accelX = rawAccelX * COEF_ACCEL;
            double accelY = rawAccelY * COEF_ACCEL;
            double accelZ = rawAccelZ * COEF_ACCEL;
            
            System.out.printf("[%d-%d] Bit1: 含重力加速度\n", offset - 6, offset - 1);
            System.out.printf("  原始值: X=0x%04X(%d), Y=0x%04X(%d), Z=0x%04X(%d)\n", 
                rawAccelX & 0xFFFF, rawAccelX, 
                rawAccelY & 0xFFFF, rawAccelY, 
                rawAccelZ & 0xFFFF, rawAccelZ);
            System.out.printf("  转换后: X=%.4f m/s², Y=%.4f m/s², Z=%.4f m/s²\n", accelX, accelY, accelZ);
            System.out.println();
        }
        
        // 6. Bit2: 角速度 (6字节)
        if ((subscriptionId & 0x0004) != 0) {
            int rawGyroX = readInt16LE(data, offset);
            offset += 2;
            int rawGyroY = readInt16LE(data, offset);
            offset += 2;
            int rawGyroZ = readInt16LE(data, offset);
            offset += 2;
            
            double gyroX = rawGyroX * COEF_GYRO;
            double gyroY = rawGyroY * COEF_GYRO;
            double gyroZ = rawGyroZ * COEF_GYRO;
            
            System.out.printf("[%d-%d] Bit2: 角速度\n", offset - 6, offset - 1);
            System.out.printf("  原始值: X=0x%04X(%d), Y=0x%04X(%d), Z=0x%04X(%d)\n", 
                rawGyroX & 0xFFFF, rawGyroX, 
                rawGyroY & 0xFFFF, rawGyroY, 
                rawGyroZ & 0xFFFF, rawGyroZ);
            System.out.printf("  转换后: X=%.3f °/s, Y=%.3f °/s, Z=%.3f °/s\n", gyroX, gyroY, gyroZ);
            System.out.println();
        }
        
        // 7. Bit3-5: 跳过
        if ((subscriptionId & 0x0008) != 0) {
            System.out.printf("[%d-%d] Bit3: 磁场 (跳过)\n", offset, offset + 5);
            offset += 6;
        }
        if ((subscriptionId & 0x0010) != 0) {
            System.out.printf("[%d-%d] Bit4: 温度/气压/高度 (跳过)\n", offset, offset + 7);
            offset += 8;
        }
        if ((subscriptionId & 0x0020) != 0) {
            System.out.printf("[%d-%d] Bit5: 四元数 (跳过)\n", offset, offset + 7);
            offset += 8;
        }
        
        // 8. Bit6: 欧拉角 (6字节) ⭐⭐⭐
        if ((subscriptionId & 0x0040) != 0) {
            int rawRoll = readInt16LE(data, offset);
            offset += 2;
            int rawPitch = readInt16LE(data, offset);
            offset += 2;
            int rawYaw = readInt16LE(data, offset);
            offset += 2;
            
            double roll = rawRoll * COEF_EULER;
            double pitch = rawPitch * COEF_EULER;
            double yaw = rawYaw * COEF_EULER;
            
            System.out.println("========================================");
            System.out.printf("[%d-%d] ⭐ Bit6: 欧拉角 ⭐\n", offset - 6, offset - 1);
            System.out.printf("  原始值: Roll=0x%04X(%d), Pitch=0x%04X(%d), Yaw=0x%04X(%d)\n", 
                rawRoll & 0xFFFF, rawRoll, 
                rawPitch & 0xFFFF, rawPitch, 
                rawYaw & 0xFFFF, rawYaw);
            System.out.printf("  转换系数: %.12f\n", COEF_EULER);
            System.out.printf("  转换后:\n");
            System.out.printf("    Roll (angleX):  %.3f°\n", roll);
            System.out.printf("    Pitch (angleY): %.3f°\n", pitch);
            System.out.printf("    Yaw (angleZ):   %.3f°\n", yaw);
            System.out.println("========================================");
            System.out.println();
            
            // 官方结果对比
            System.out.println("========== 官方结果对比 ==========");
            System.out.printf("  官方 angleX (Roll):  -90.276°\n");
            System.out.printf("  我们 Roll:           %.3f°\n", roll);
            System.out.printf("  差异:                %.3f°\n", Math.abs(-90.276 - roll));
            System.out.println();
            System.out.printf("  官方 angleY (Pitch): 1.219°\n");
            System.out.printf("  我们 Pitch:          %.3f°\n", pitch);
            System.out.printf("  差异:                %.3f°\n", Math.abs(1.219 - pitch));
            System.out.println();
            System.out.printf("  官方 angleZ (Yaw):   -84.836°\n");
            System.out.printf("  我们 Yaw:            %.3f°\n", yaw);
            System.out.printf("  差异:                %.3f°\n", Math.abs(-84.836 - yaw));
            System.out.println("=====================================");
        }
    }
    
    private static byte[] hexStringToByteArray(String hex) {
        int len = hex.length();
        byte[] data = new byte[len / 2];
        for (int i = 0; i < len; i += 2) {
            data[i / 2] = (byte) ((Character.digit(hex.charAt(i), 16) << 4)
                                + Character.digit(hex.charAt(i + 1), 16));
        }
        return data;
    }
    
    private static int readUInt16LE(byte[] data, int offset) {
        return (data[offset] & 0xFF) | ((data[offset + 1] & 0xFF) << 8);
    }
    
    private static long readUInt32LE(byte[] data, int offset) {
        return (data[offset] & 0xFFL) | 
               ((data[offset + 1] & 0xFFL) << 8) | 
               ((data[offset + 2] & 0xFFL) << 16) | 
               ((data[offset + 3] & 0xFFL) << 24);
    }
    
    private static int readInt16LE(byte[] data, int offset) {
        int value = (data[offset] & 0xFF) | ((data[offset + 1] & 0xFF) << 8);
        // 转换为有符号16位整数
        if (value >= 32768) {
            value -= 65536;
        }
        return value;
    }
}

